#include "myapp.h"
#include "WT61.h"
#include <stdio.h>
#include "dataProcess.h"
#include "key.h" // Include the header file where Key_ESC is defined
#include "main.h" // Include GPIO header for JiGuang control



uint8_t CheckIsInZone(int16_t dx,int16_t dy,int16_t checkZone) {
    uint8_t inZone = 0; // Flag to check if within zone
    dx = (dx>=0?dx : -dx);
    dy = (dy>=0?dy : -dy);
    if (dx < checkZone && dy < checkZone) {
        inZone = 1; // Within the zone
    }
    return inZone;
    
    
}

void SetJiGuang(uint8_t OnOff) {
    static uint8_t jiGuangOn = 0; // Flag to check if JiGuang is on
    if (OnOff && !jiGuangOn) {
        jiGuangOn = 1; // Turn on JiGuang
        HAL_GPIO_WritePin(JiGuang_GPIO_Port, JiGuang_Pin, GPIO_PIN_SET); // Assuming GPIOB_PIN_0 controls JiGuang
        
    } else if (!OnOff && jiGuangOn) {
        jiGuangOn = 0; // Turn off JiGuang
        HAL_GPIO_WritePin(JiGuang_GPIO_Port, JiGuang_Pin, GPIO_PIN_RESET); // Assuming GPIOB_PIN_0 controls JiGuang

    }

}


My_StatusTypeDef task2(int key_Num) {
    displayStr("task2...ing", Display_firstCol, Display_firstRow ,0);
    task_state = 2; // Set task state to 2
    if((background_x != 0xffff||background_y!=0xffff )&&CheckIsInZone((int16_t)(background_x-jiguang_real_x), (int16_t)(background_y-jiguang_real_y), 4))
    {
        SetJiGuang(1); // Turn on JiGuang if within zone
    }
    
    
    
    if(key_Num== Key_ESC)
    {
        task_state = 0;
        SetJiGuang(0); // Turn off JiGuang
        return MY_READY; //ESC键返回
    }
    return MY_BUSY; //继续执行
}

My_StatusTypeDef task3_Ni(int key_Num) {
    displayStr("task3Shun...ing", Display_firstCol, Display_firstRow ,0);
    task_state = 3; // Set task state to 2
    static uint16_t flag=0;
    if((background_x != 0xffff||background_y!=0xffff )&&CheckIsInZone((int16_t)(background_x-jiguang_real_x), (int16_t)(background_y-jiguang_real_y), 4))
    {
        SetJiGuang(1); // Turn on JiGuang if within zone
        flag = 1;
    }
    if (!flag && background_x == 0xffff && background_y == 0xffff) // If background coordinates are not set
    {
        Ni_find=1;
    }
    else
    {
        Ni_find=0;
    }
    

    if(key_Num== Key_ESC)
    {
        task_state = 0;
        Ni_find=0;
        flag=0;
        SetJiGuang(0); // Turn off JiGuang
        return MY_READY; //ESC键返回
    }
    return MY_BUSY; //继续执行
}

My_StatusTypeDef task3_Shun(int key_Num) {
    displayStr("task3Ni...ing", Display_firstCol, Display_firstRow ,0);
    task_state = 3; // Set task state to 2
    static uint16_t flag=0;
    if((background_x != 0xffff||background_y!=0xffff )&&CheckIsInZone((int16_t)(background_x-jiguang_real_x), (int16_t)(background_y-jiguang_real_y), 4))
    {
        SetJiGuang(1); // Turn on JiGuang if within zone
        flag = 1;
    }

    if (!flag && background_x == 0xffff && background_y == 0xffff) // If background coordinates are not set
    {
        Shun_find=1;
    }
    else
    {
        Shun_find=0;
    }

    

    if(key_Num== Key_ESC)
    {
        task_state = 0;
        Shun_find=0;
        flag=0;
        SetJiGuang(0); // Turn off JiGuang
        return MY_READY; //ESC键返回
    }
    return MY_BUSY; //继续执行
}

My_StatusTypeDef configureSetJiGuangX(int key_Num)
{
    displayNum(jiguang_real_x, Display_firstCol, Display_firstRow);
    SetJiGuang(1); // Turn on JiGuang for configuration
    task_state = 1;
    if(key_Num== Key_ESC)
    {
        SetJiGuang(0); // Turn off JiGuang
        task_state = 0;
        
        return MY_READY; //ESC键返回
    }
    else if(key_Num == Key_Down)
    {
        AddJiGuangRealXY(-1, 0,0); //向下移动
    }
    else if(key_Num == Key_UP)
    {
        AddJiGuangRealXY(1, 0,0); //向上移动
    }
    else if(key_Num == Key_OK)
    {
        //存儲
        // if(myFlash_read(FLASH_START_ADDR) != jiguang_real_x) {
            uint32_t data[3];
            data[0] = jiguang_real_x;
            data[1] = jiguang_real_y;
            data[2] = find_speed;
            myFlash_write(CONFIGURE_SET_JIGUANG_X, data, 3); // Save the configuration
        // }
        return MY_OK; //确认设置
    }
    return MY_BUSY; //继续执行
}

My_StatusTypeDef configureSetJiGuangY(int key_Num)
{
    displayNum(jiguang_real_y, Display_firstCol, Display_firstRow);
    SetJiGuang(1); // Turn on JiGuang for configuration
    task_state = 1;
    if(key_Num== Key_ESC)
    {
        SetJiGuang(0); // Turn off JiGuang
        task_state = 0;
        return MY_READY; //ESC键返回
    }
    else if(key_Num == Key_Down)
    {
        AddJiGuangRealXY(0, -1 ,0); //向下移动
    }
    else if(key_Num == Key_UP)
    {
        AddJiGuangRealXY(0, 1 , 0); //向上移动
    }
    else if(key_Num == Key_OK)
    {
        //存儲
        // if(myFlash_read(FLASH_START_ADDR) != jiguang_real_y) {
            // myFlash_write(CONFIGURE_SET_JIGUANG_Y, (uint32_t *)&jiguang_real_y, 1); // Save the configuration
        // }

        uint32_t data[3];
            data[0] = jiguang_real_x;
            data[1] = jiguang_real_y;
            data[2] = find_speed;
            myFlash_write(CONFIGURE_SET_JIGUANG_X, data, 3); // Save the configuration
        return MY_OK; //确认设置
    }
    return MY_BUSY; //继续执行
}

My_StatusTypeDef configureSetFindSpeed(int key_Num)
{
    displayNum(find_speed, Display_firstCol, Display_firstRow);
    if(key_Num== Key_ESC)
    {
        return MY_READY; //ESC键返回
    }
    else if(key_Num == Key_Down)
    {
        AddJiGuangRealXY(0, 0,-1); //向下移动
    }
    else if(key_Num == Key_UP)
    {
        AddJiGuangRealXY(0, 0, 1); //向上移动
    }
    else if(key_Num == Key_OK)
    {
        //存儲
        // if(myFlash_read(FLASH_START_ADDR) != jiguang_real_y) {
            // myFlash_write(CONFIGURE_SET_JIGUANG_Y, (uint32_t *)&jiguang_real_y, 1); // Save the configuration
        // }

        uint32_t data[3];
            data[0] = jiguang_real_x;
            data[1] = jiguang_real_y;
            data[2] = find_speed;
            myFlash_write(CONFIGURE_SET_JIGUANG_X, data, 3); // Save the configuration
        return MY_OK; //确认设置
    }
    return MY_BUSY; //继续执行
}


My_StatusTypeDef gyro_show(int key_Num) {
    char buff[15];
    sprintf(buff, "x: %f", stcAngle.Angle[0]);
    displayStr(buff, Display_firstCol, Display_firstRow, 0);
    sprintf(buff, "y: %f", stcAngle.Angle[1]);
    displayStr(buff, Display_firstCol, Display_secondRow, 0);
    sprintf(buff, "z: %f", stcAngle.Angle[2]);
    displayStr(buff, Display_firstCol, Display_thirdRow, 0);
    if (key_Num == Key_ESC) {
      return MY_READY;
    } else if (key_Num == Key_OK) {
      return MY_STANDBY;
    } else if (key_Num == Key_Down) {
    } else if (key_Num == Key_UP) {
    }
  
    return MY_BUSY;
  }




